/* Matrix.cc -- Objeto Matriz
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdlib.h>
#include <string.h>
#include <iostream>

#include "Matrix.hh"

using namespace std;

Matrix::Matrix( const int n_row, const int n_col)
{
    int i = 0;

    n_row_ = n_row;
    n_col_ = n_col;

    matrix_ = (float**) malloc( sizeof(float *) * n_row );
    for( i = 0 ; i < n_row_ ; i++ )
    {
	matrix_[i] = (float*) malloc( sizeof(float) * n_col_ );
	memset( matrix_[i], 0, sizeof(float) * n_col_ );
    }
}

void
Matrix::multRaw( const int n_row_A, const int n_col_A, const float ** matrix_A,
		 const int n_row_X, const int n_col_X, const float ** matrix_X,
		 float ** matrix_B )
{
    int i = 0;
    int j = 0;
    int k = 0;

    for( i = 0 ; i < n_row_A ; i++ )
	for( j = 0 ; j < n_col_X ; j++ )
	    for( k = 0 ; k < n_col_A ; k++ )
		matrix_B[i][j] += matrix_A[i][k] * matrix_X[k][j];
}

void
Matrix::mult( Matrix * matrix_A,
	      Matrix * matrix_X,
	      Matrix * matrix_B )
{
    int i = 0;
    int j = 0;
    int k = 0;

    for( i = 0 ; i < matrix_A->getNRow() ; i++ )
	for( j = 0 ; j < matrix_X->getNCol() ; j++ )
	    for( k = 0 ; k < matrix_A->getNCol() ; k++ )
		matrix_B->set( i, j, matrix_B->get(i,j) + (matrix_A->get(i,k) * matrix_X->get(k,j)));
}

/** Limpa a matriz inserindo 0s.

    @param[in][out] *matrix Matriz a ser limpa.
*/
void
Matrix::clean()
{
    int i = 0;

    for( i = 0 ; i < n_row_ ; i++ )
	memset( matrix_[i], 0, sizeof(float) * n_col_ );
}

/** Imprime uma matriz.

    @param *mat ponteiro para uma matriz.
*/
void
Matrix::print()
{
    int i = 0;
    int j = 0;

    for( i = 0 ; i < n_row_ ; i++ )
    {
	for( j = 0 ; j < n_col_ ; j++ )
	    cout << matrix_[i][j] << "  ";
	cout << endl;
    }
}

/** Seta o valor em uma posição na matriz.
 *
 * @param *mat Matriz.
 *
 * @param x Linha.
 *
 * @param y Coluna.
 *
 * @param value Novo valor.
 */
void
Matrix::set( const int x, const int y, const float value )
{
    matrix_[x][y] = value;
}

/** Retorna o valor contido na posição passada.
 *
 * @param *mat Matriz.
 *
 * @param x Linha.
 *
 * @param y Coluna.
 *
 * @return O valor contido na posição requisitada.
 */
float
Matrix::get( const int x, const int y )
{
    return matrix_[x][y];
}

int
Matrix::getNRow()
{
    return n_row_;
}

int
Matrix::getNCol()
{
    return n_col_;
}
